package behavior;

import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.TachoPilot;
import lejos.robotics.subsumption.Behavior;
/**
 * This behavior acts when the ultrasonicsensor detects an object withing 10 cm.
 * The robot will turn right with intervals of 10 degree until a total of 90. If it reaches 90 degrees, it turns back to its beginposition
 * and starts rotating left. The behaviour stops when there's no longer an object within 10 cm detected.
 */
public class ProximityDetector implements Behavior {

	private TachoPilot pilot;
	private UltrasonicSensor sensor;
	private boolean isSupressed=false;
	
	public ProximityDetector(TachoPilot pilot, UltrasonicSensor sensor) throws Exception
	{
		this.pilot = pilot;
		this.sensor = sensor;
		if (sensor.continuous()<0) 
			throw new Exception();
	}
	
	@Override
	public void action() {
		isSupressed = false;
		
		float angle = 0;
		float angleIncr = 10;
		
		while (!isSupressed && sensor.getDistance()<10){
			
			if(angle >= 90){
				angle = 0;
				angleIncr = -10;
				pilot.rotate(-90);
			}
			
			pilot.rotate(angle);
			angle +=angleIncr;			
		}
	}

	@Override
	public void suppress() {
		isSupressed = true;		
	}

	@Override
	public boolean takeControl() {
		//in cm
		return sensor.getDistance()<10;
	}

}
